#pragma once

#include "head.hpp"
#include "sensor.hpp"


class groundFunc{
public:
    groundFunc(){}
    ~groundFunc(){}
    virtual void evaluate(double x, double y, double & z){}
};

class groundsin: public groundFunc{
public:
    groundsin(){}
    ~groundsin(){}
    virtual void evaluate(double x, double y, double & z) override{
        z  = 0.2 * sin(x) * cos(y);
    }
};

class robots{
public:
    robots(){}
    // 通过传进来的参数算出轮子的位置
    virtual void getWheelPos(Eigen::Matrix3d midPos, Eigen::Vector3d midPoint, std::vector<Eigen::Vector3d> & wheelTrans){}
    ~robots(){}    
};

// 定义三个轮子的机器人
// ×××××××O××××××××
// ××××××××××××××××
// ××××××××××××××××
// ×O×××××××××××O×
class threeWheelRobots: public robots{
public:
    std::vector<Eigen::Vector3d> wheelInbody;
    
    laser laserModule;// 激光模型类

    threeWheelRobots(){
        wheelInbody.push_back(Eigen::Vector3d(0.2, 0., -0.1)); 
        wheelInbody.push_back(Eigen::Vector3d(-0.1, -std::sqrt(3.)* 0.1, -0.1));
        wheelInbody.push_back(Eigen::Vector3d(-0.1, std::sqrt(3.) * 0.1, -0.1));

        setLaserPosAndTrans();
    }

    // 设置激光墙部分
    void setLaserWalls(std::vector<Eigen::Vector3d> _walls){
        laserModule.setWall(_walls);
    }
    ~threeWheelRobots(){}
    // 得到轮子的位置
    virtual void getWheelPos(Eigen::Matrix3d midPos, Eigen::Vector3d midPoint, std::vector<Eigen::Vector3d> & wheelTrans) override{
        wheelTrans.clear();
        for(auto & wheel_x : wheelInbody){
            Eigen::Vector3d tw_wheel = midPos * wheel_x  + midPoint;
            wheelTrans.push_back(tw_wheel);
        }
    }
    // 定义小车上有四个激光器0 1照射下面  2 3号照射左面
    std::vector<Eigen::Matrix3d> mLaserPos;
    std::vector<Eigen::Vector3d> mLaserTrans;
    
    // 设置4个激光器在机器人坐标系位置和姿态
    void setLaserPosAndTrans(){
        // 设置位置
        mLaserTrans.push_back(Eigen::Vector3d(-0.3, -0.2, 0.));
        mLaserTrans.push_back(Eigen::Vector3d(-0.3, 0.2, 0.));

        mLaserTrans.push_back(Eigen::Vector3d(-0.2, 0.3, 0.));
        mLaserTrans.push_back(Eigen::Vector3d(0.2, 0.3, 0.));
        // 设置姿态
        Eigen::AngleAxisd rot270(-M_PI*1.5, Vector3d(0, 0, 1));
        Eigen::AngleAxisd rot180(-M_PI, Vector3d(0, 0, 1));
        Eigen::Matrix3d run270 = rot270.matrix();
        Eigen::Matrix3d run180 = rot180.matrix();

        mLaserPos.push_back(run180);
        mLaserPos.push_back(run180);
        mLaserPos.push_back(run270);
        mLaserPos.push_back(run270);

    }
    // 得到一组2d的ps数据
    void get2dLaserPosAndTrans(std::vector<Eigen::Vector3d> & _get){
        _get.clear();
        // 设置位置

        Eigen::Vector3d p0(-0.3, -0.2, -M_PI);
        Eigen::Vector3d p1(-0.3, 0.2, -M_PI);
        Eigen::Vector3d p2(-0.2, 0.3, -M_PI*1.5);
        Eigen::Vector3d p3(0.2, 0.3, -M_PI*1.5);
        // 设置姿态
        _get.push_back(p0);
        _get.push_back(p1);
        _get.push_back(p2);
        _get.push_back(p3);
    } 
    // 对四个激光器求解距离 输入的是小车的点 3 3 3 分别是  
    // 激光测量点在世界坐标系的点  
    // 激光测量点在laser坐标系的点
    // 激光点在世界坐标系的位置

    std::vector<Eigen::Vector9d> getLaserData(Eigen::Matrix3d midPos, Eigen::Vector3d midPoint){
        std::vector<Eigen::Vector9d> resVec;
        for(size_t i = 0; i < mLaserPos.size(); i++){
            Eigen::Matrix3d laserPosInWorld = midPos * mLaserPos[i];// 激光在世界坐标系下的位姿
            Eigen::Vector3d transInworld = midPos * mLaserTrans[i]   + midPoint;// 在世界最标系的位置

            Eigen::Vector6d res = laserModule.measure(laserPosInWorld, transInworld);// 求得现在的位置

            Eigen::Vector9d temp;
            temp.block<6,1>(0, 0) = res;
            temp.block<3,1>(6, 0) = transInworld;// 激光点在世界坐标系的位置
            resVec.push_back(temp);
        }
        return resVec;
    }
    // 验证雷达的方向
    std::vector<Eigen::Vector6d> getLaserDirection(Eigen::Matrix3d midPos, Eigen::Vector3d midPoint){
        std::vector<Eigen::Vector6d> resVec;
        for(size_t i = 0; i < mLaserPos.size(); i++){
            Eigen::Matrix3d laserPosInWorld = midPos * mLaserPos[i];// 激光在世界坐标系下的位姿 Rw_c * Rc_l 
            Eigen::Vector3d transInworld = midPos * mLaserTrans[i] + midPoint;// 在世界最标系的位置
            Eigen::Vector3d mes(0.1, 0., 0.);// 假设测量了0.1 m
            Eigen::Vector3d dirInWorld = laserPosInWorld * mes + transInworld;// 推算出方向位置
            Eigen::Vector6d temp;
            temp.block<3,1>(0, 0) = transInworld;
            temp.block<3,1>(3, 0) = dirInWorld;// 激光点在世界坐标系的位置 方向
            resVec.push_back(temp);
        }
        return resVec;
    }
};
